/*!
  @file motion_solver.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _MOTION_SOLVER_H_
#define _MOTION_SOLVER_H_

#include <vector>

#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <opencv2/core/core.hpp>
#include <ros/ros.h>

#include "mrover_vo/camera_params.h"
#include "mrover_vo/feature.h"


namespace vo {

/*! 
 *  @brief  Motion solver base class
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  this class provides the interfaces for motion solvers
 */

class MotionSolver {
 protected:
  typedef boost::shared_ptr<Feature::KeyPoints const> KeyPointsConstPtr;
  typedef boost::shared_ptr<Feature::Descriptors const> DescriptorsConstPtr;
  typedef boost::shared_ptr<Feature::Matches const> MatchesConstPtr;
  typedef std::vector<int> Indices;
  typedef boost::shared_ptr<Indices> IndicesPtr;

 public:
  typedef boost::scoped_ptr<MotionSolver> Ptr;


  MotionSolver() 
      : inliers_(new Indices)
  { }

  virtual ~MotionSolver() { };

  void setInputFeature(const Feature::ConstPtr &feature) { input_ = feature; }
  void setTargetFeature(const Feature::ConstPtr &feature) { target_ = feature; }
  void setInputMatch(const MatchesConstPtr &matches) { matches_ = matches; }
  void setOutputPose(cv::Mat &pose) { pose_ = pose; }

  void getInlierIndices(Indices &inliers) const { inliers = *inliers_; }

  void setCameraParams(const CameraParams::ConstPtr &cam_params) {
    cam_params_ = cam_params;
    copyCameraParams();
  }

  virtual inline void compute(cv::Mat &pose) {
    setOutputPose(pose);
    compute();
  }

  virtual inline void compute(cv::Mat &pose, Indices &inliers) {
    setOutputPose(pose);
    compute();
    getInlierIndices(inliers);
  }

  virtual void compute() = 0;
  virtual inline void computeLM(cv::Mat &pose) { }

 protected:
  virtual inline bool initCompute() {
    if (!input_ || !target_ || !matches_ || !pose_.data || !inliers_) return false;
    return true;
  }

  //! copies the camera params if necessary
  virtual void copyCameraParams() {}


  CameraParams::ConstPtr cam_params_;

  Feature::ConstPtr input_;
  Feature::ConstPtr target_;
  MatchesConstPtr matches_;
  cv::Mat pose_;
  IndicesPtr inliers_;
};

}  // namespace vo

#endif
